#include "gpio.h"
#include "Motor.h"
#include "Encoder.h"
#include "math.h"
#include "tim.h"

Motor_Speed_Ctrl_t Motor_Speed_Ctrl[4];
float Motor_PWM[4]={0};

float test=0;

//test
int16_t pulse_diff;

/* 
  *PWM�źž���TIM8����

  //ʹ��
  STBY  ---->PE8    (����)
	
	//����,PB1,PF1�ò���
	PWMA  ---->PC6     �����ԣ�
	AIN1  ---->PB8     �����ԣ�
	AIN2  ---->PF11    �����ԣ�
	E1A   ---->PA8     TIM1
	E1B   ---->PE11    TIM1
	
	//����
	PWMB  ---->PC7     �����ԣ�
	BIN1  ---->PC4     �����ԣ�
	BIN2  ---->PC5      �����ԣ�
	E2A   ---->PA0     TIM5
	E2B   ---->PA1    TIM5

	//����
	PWMC  ---->PC8     �����ԣ�
	CIN1  ---->PA4     (����)
	CIN2  ---->PA5      �����ԣ�
	E3A   ---->PB4     TIM3
	E3B   ---->PB5     TIM3

	//����
	PWMD  ---->PC9     �����ԣ�
	DIN1  ---->PC2      �����ԣ�
	DIN2  ---->PC3      �����ԣ�
	E4A   ---->PD12    TIM4
	E4B   ---->PD13    TIM4
	
	
	ǰ
    A-----D
	|     |
	|     |    
	| car |
	|     |
	|     |
	B-----C
    ��

*/

// 全局电机配置表
MotorHashEntry motor_hash_table[4] = {
    // 电机A: PB8高, PF11低 正转
    {
        {GPIOB, GPIO_PIN_8, GPIOF, GPIO_PIN_11},
        {GPIO_PIN_SET, GPIO_PIN_RESET, GPIO_PIN_RESET, GPIO_PIN_SET}
    },
    // 电机B: PC4低, PC5高 正转
    {
        {GPIOC, GPIO_PIN_4, GPIOC, GPIO_PIN_5},
        {GPIO_PIN_RESET, GPIO_PIN_SET, GPIO_PIN_SET, GPIO_PIN_RESET}
    },
    // 电机C: PA4低, PA5高 正转
    {
        {GPIOA, GPIO_PIN_4, GPIOA, GPIO_PIN_5},
        {GPIO_PIN_RESET, GPIO_PIN_SET, GPIO_PIN_SET, GPIO_PIN_RESET}
    },
    // 电机D: PC2高, PC3低 正转
    {
        {GPIOC, GPIO_PIN_2, GPIOC, GPIO_PIN_3},
        {GPIO_PIN_SET, GPIO_PIN_RESET, GPIO_PIN_RESET, GPIO_PIN_SET}
    }
};

// ʹ�ܵ��
void Motor_Enable(void)
{
    HAL_GPIO_WritePin(GPIOE, GPIO_PIN_8, GPIO_PIN_SET); // STBY = 1
	
	  //A
    HAL_GPIO_WritePin(GPIOB,GPIO_PIN_8,GPIO_PIN_RESET);
    HAL_GPIO_WritePin(GPIOF,GPIO_PIN_11,GPIO_PIN_SET);
    //B
    HAL_GPIO_WritePin(GPIOC,GPIO_PIN_4,GPIO_PIN_RESET);
    HAL_GPIO_WritePin(GPIOC,GPIO_PIN_5,GPIO_PIN_SET);
    //C
    HAL_GPIO_WritePin(GPIOA,GPIO_PIN_4,GPIO_PIN_RESET);
    HAL_GPIO_WritePin(GPIOA,GPIO_PIN_5,GPIO_PIN_SET);
    //D
    HAL_GPIO_WritePin(GPIOC,GPIO_PIN_2,GPIO_PIN_SET);
    HAL_GPIO_WritePin(GPIOC,GPIO_PIN_3,GPIO_PIN_RESET);
}

// �������,�������
void Motor_Disable(void)
{
    HAL_GPIO_WritePin(GPIOE, GPIO_PIN_8, GPIO_PIN_RESET); // STBY = 0
}

/**
 * @brief ��������ٶȼ���
 * ������ǰ��ʱ������ת��,��������ֵ���ϼ�С
 * ������������ֵ�仯��Χ-32768~32768
 */
void Motor_Calculate_Speed(int wheel_index)
{
	
	  if(wheel_index < 0 || wheel_index > 3) return;
    
    Encoder_State_t *encoder = &encoder_state[wheel_index];
    uint32_t current_time = HAL_GetTick();
    uint32_t time_diff = current_time - encoder->last_update_time;
    
    if(time_diff < 10) return;  // ??????????10ms
    
    // ???�????
    encoder->last_count = encoder->current_count;
    encoder->current_count = (int16_t)__HAL_TIM_GET_COUNTER(encoder->htim);
    
    // ??????????
    //int16_t pulse_diff = encoder->current_count - encoder->last_count;
	 //test
	 pulse_diff = encoder->current_count - encoder->last_count;
    
    // ??????? (RPM)
    float time_seconds = time_diff / 1000.0f;
    float pulses_per_rev = 4.0f * 13.0f * 30.0f;  // ??????????????
    encoder->speed_rpm = (pulse_diff / pulses_per_rev) * (60.0f / time_seconds); //????�???????????
    
    encoder->last_update_time = current_time;
		
}

// ???????????????
void Update_All_Speed(void)
{
    for(int i = 0; i < 4; i++) {
        Motor_Calculate_Speed(i);
    }
}

// ??????????
float Get_Wheel_Speed(int wheel_index)
{
    if(wheel_index < 0 || wheel_index > 3) return 0.0f;
    return encoder_state[wheel_index].speed_rpm;
}



//��ʼ�����Ӳ���
void MotorInit(int Motor_ID)
{
  Motor_Speed_Ctrl[Motor_ID].actual_speed=0;
	Motor_Speed_Ctrl[Motor_ID].target_speed=0;

	Motor_Speed_Ctrl[Motor_ID].err_last=0;
	Motor_Speed_Ctrl[Motor_ID].err_now=0;
	Motor_Speed_Ctrl[Motor_ID].err_sum=0;

	Motor_Speed_Ctrl[Motor_ID].ki=0.001;//������ 0.01
	Motor_Speed_Ctrl[Motor_ID].kp=0.3;//������ 4
	Motor_Speed_Ctrl[Motor_ID].kd=0.5;//������ 1

	Motor_Speed_Ctrl[Motor_ID].acc=0;
	Motor_Speed_Ctrl[Motor_ID].dec=0;

	Motor_Speed_Ctrl[Motor_ID].i_out=0;
	Motor_Speed_Ctrl[Motor_ID].p_out=0;
	Motor_Speed_Ctrl[Motor_ID].d_out=0;
	Motor_Speed_Ctrl[Motor_ID].output=0;

	Motor_Speed_Ctrl[Motor_ID].maxOutput =950;  // ����޷�
}

/**
 * @brief  �������
 * @note   ����Ƶ����Ҫ����ʵ���������
 */
void MotorCtrl(void)
{
	for(int i=0;i<=3;i++)
	{
		MotorPidCtrl(&Motor_Speed_Ctrl[i],i);
	}

	//��rpmת��Ϊpwm
	for(int i=0;i<=3;i++)
	{
		Motor_PWM[i] = RPM_To_PWM(Motor_Speed_Ctrl[i].output);
    Motor_Control_Hash(i);
	}
	

	 Set_PWM_ALL();

}

//���PID����
void MotorPidCtrl(Motor_Speed_Ctrl_t *MotorPid,int Motor_ID)
{
    
	MotorPid->actual_speed =  Get_Wheel_Speed(Motor_ID);
	MotorPid->err_now = MotorPid->target_speed - MotorPid->actual_speed;
	MotorPid->err_sum += MotorPid->err_now;
	MotorPid->err_sum=MaxMinLimit(MotorPid->err_sum, 100);

    //�������Ƶ���
	MotorPid->p_out = MotorPid->err_now * MotorPid->kp;

    //���ֿ��Ƶ���
	MotorPid->i_out = MotorPid->err_sum * MotorPid->ki; 
	//�����޷�����ֹ���ֱ���
    MotorPid->i_out = MaxMinLimit(MotorPid->i_out, MotorPid->maxOutput); 

    //΢�ֿ��Ƶ���
    MotorPid->d_out = (MotorPid->err_now - MotorPid->err_last) * MotorPid->kd; 

	MotorPid->err_last = MotorPid->err_now;

	//�������
    MotorPid->output = MotorPid->p_out + MotorPid->i_out + MotorPid->d_out;
	//����޷�
    MotorPid->output = MaxMinLimit(MotorPid->output, MotorPid->maxOutput);


}

/**
 * @brief  max min limit
 * @param  inDat:
 * @retval outDat
 */
float MaxMinLimit(float val, float limit)
{
    if (val > limit)
        val = limit;
    if (val < -limit)
        val = -limit;

    return val;
}

//�ú������������������������ת��RPM
void SetSpeed(int Motor_ID, float rpm)
{
	Motor_Speed_Ctrl[Motor_ID].target_speed = rpm;		
}

/**
 * @brief ��ת��(RPM)ת��ΪPWMռ�ձȣ������������ͷ�����У����
 * @param rpm: Ŀ��ת��(RPM)
 * @param max_rpm: ������ת��
 * @param max_pwm: PWM���ֵ
 * @param dead_zone_rpm: ��������ת��(�����ʼת������СRPM)
 * @return PWMռ�ձ�ֵ
 */
float RPM_To_PWM(float rpm)
{
    // ����ת�ٷ�Χ
    rpm = MaxMinLimit(rpm, max_rpm);
	
    //����ת�ٺ�PWM������
    float normalized_rpm = rpm / max_rpm;
    // ����PWM
    float pwm = normalized_rpm * max_pwm;
    
    return pwm;
}

void Set_PWM_ALL()
{
	
	__HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_1,Motor_PWM[0]);
	__HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_2,Motor_PWM[1]);
	__HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_3,Motor_PWM[2]);
	__HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_4,Motor_PWM[3]);

//	__HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_1,50);
//	__HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_2,50);
//	__HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_3,4);
//	__HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_4,4);
}



// 使用哈希表风格的电机控制函数
void Motor_Control_Hash(int Motor_ID)
{
   
        MotorHashEntry* motor = &motor_hash_table[Motor_ID];
        
        if (Motor_PWM[Motor_ID] >= 0)
				{
            // 正转
            HAL_GPIO_WritePin(motor->pin_config.in1_port, motor->pin_config.in1_pin, 
                             motor->direction.in1_forward);
            HAL_GPIO_WritePin(motor->pin_config.in2_port, motor->pin_config.in2_pin, 
                             motor->direction.in2_forward);
        } 
				else 
				{
            // 反转
            HAL_GPIO_WritePin(motor->pin_config.in1_port, motor->pin_config.in1_pin, 
                             motor->direction.in1_reverse);
            HAL_GPIO_WritePin(motor->pin_config.in2_port, motor->pin_config.in2_pin, 
                             motor->direction.in2_reverse);
					
					Motor_PWM[Motor_ID]=-Motor_PWM[Motor_ID];
        }
				
    
}

//void Motor_Control_Optimized(float motor_pwm[4])
//{
//    // 定义电机控制结构
//    typedef struct {
//        GPIO_TypeDef* in1_port;
//        uint16_t in1_pin;
//        GPIO_TypeDef* in2_port;
//        uint16_t in2_pin;
//        void (*set_pwm)(int32_t pwm); // PWM设置函数指针
//    } Motor_Config;
//    
//    // 电机配置数组
//    Motor_Config motors[4] = {
//        // 电机A: 正转时 PB8高, PF11低
//        {GPIOB, GPIO_PIN_8, GPIOF, GPIO_PIN_11, NULL},
//        // 电机B: 正转时 PC4低, PC5高  
//        {GPIOC, GPIO_PIN_4, GPIOC, GPIO_PIN_5, NULL},
//        // 电机C: 正转时 PA4低, PA5高
//        {GPIOA, GPIO_PIN_4, GPIOA, GPIO_PIN_5, NULL},
//        // 电机D: 正转时 PC2高, PC3低
//        {GPIOC, GPIO_PIN_2, GPIOC, GPIO_PIN_3, NULL}
//    };
//    
//    for(int i = 0; i < 4; i++)
//    {
//        if(motor_pwm[i] >= 0)
//        {
//            // 正转控制
//            switch(i)
//            {
//                case 0: // 电机A
//                    HAL_GPIO_WritePin(motors[i].in1_port, motors[i].in1_pin, GPIO_PIN_SET);
//                    HAL_GPIO_WritePin(motors[i].in2_port, motors[i].in2_pin, GPIO_PIN_RESET);
//                    break;
//                case 1: // 电机B
//                    HAL_GPIO_WritePin(motors[i].in1_port, motors[i].in1_pin, GPIO_PIN_RESET);
//                    HAL_GPIO_WritePin(motors[i].in2_port, motors[i].in2_pin, GPIO_PIN_SET);
//                    break;
//                case 2: // 电机C
//                    HAL_GPIO_WritePin(motors[i].in1_port, motors[i].in1_pin, GPIO_PIN_RESET);
//                    HAL_GPIO_WritePin(motors[i].in2_port, motors[i].in2_pin, GPIO_PIN_SET);
//                    break;
//                case 3: // 电机D
//                    HAL_GPIO_WritePin(motors[i].in1_port, motors[i].in1_pin, GPIO_PIN_SET);
//                    HAL_GPIO_WritePin(motors[i].in2_port, motors[i].in2_pin, GPIO_PIN_RESET);
//                    break;
//            }

//        }
//        else
//        {
//            // 反转控制
//            switch(i)
//            {
//                case 0: // 电机A
//                    HAL_GPIO_WritePin(motors[i].in1_port, motors[i].in1_pin, GPIO_PIN_RESET);
//                    HAL_GPIO_WritePin(motors[i].in2_port, motors[i].in2_pin, GPIO_PIN_SET);
//                    break;
//                case 1: // 电机B
//                    HAL_GPIO_WritePin(motors[i].in1_port, motors[i].in1_pin, GPIO_PIN_SET);
//                    HAL_GPIO_WritePin(motors[i].in2_port, motors[i].in2_pin, GPIO_PIN_RESET);
//                    break;
//                case 2: // 电机C
//                    HAL_GPIO_WritePin(motors[i].in1_port, motors[i].in1_pin, GPIO_PIN_SET);
//                    HAL_GPIO_WritePin(motors[i].in2_port, motors[i].in2_pin, GPIO_PIN_RESET);
//                    break;
//                case 3: // 电机D
//                    HAL_GPIO_WritePin(motors[i].in1_port, motors[i].in1_pin, GPIO_PIN_RESET);
//                    HAL_GPIO_WritePin(motors[i].in2_port, motors[i].in2_pin, GPIO_PIN_SET);
//                    break;
//            }

//        }
//    }
//}






